/*
Copyright 2007 Luke Benstead

This file is part of slMath.

slMath is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser Public License as published by
the Free Software Foundation; either version 3 of the License, or
(at your option) any later version.

slMath is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser Public License for more details.

You should have received a copy of the GNU Lesser Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#ifdef __CPLUSPLUS
#include <cassert>
#include <memory>
#else
#include <assert.h>
#include <memory.h>
#endif

#include "math/utility.h"
#include "math/mat4.h"
#include "math/vec3.h"
#include "math/quaternion.h"

///< Returns pOut, sets pOut to the conjugate of pIn
slQuaternion* slQuaternionConjugate(slQuaternion* pOut, const slQuaternion* pIn)
{
	pOut->m_X = -pIn->m_X;
	pOut->m_Y = -pIn->m_Y;
	pOut->m_Z = -pIn->m_Z;
	pOut->m_W = pIn->m_W;

	return pOut;
}

///< Returns the dot product of the 2 quaternions
slScalar slQuaternionDot(const slQuaternion* q1, const slQuaternion* q2)
{
	// A dot B = B dot A = AtBt + AxBx + AyBy + AzBz

	return (q1->m_W * q2->m_W +
			q1->m_X * q2->m_X +
			q1->m_Y * q2->m_Y +
			q1->m_Z * q2->m_Z);
}

///< Returns the exponential of the quaternion
slQuaternion* slQuaternionExp(slQuaternion* pOut, const slQuaternion* pIn)
{
	assert(0);

	return pOut;
}

///< Makes the passed quaternion an identity quaternion
slQuaternion* slQuaternionIdentity(slQuaternion* pOut)
{
	pOut->m_X = 0.0;
	pOut->m_Y = 0.0;
	pOut->m_Z = 0.0;
	pOut->m_W = 1.0;

	return pOut;
}

///< Returns the inverse of the passed Quaternion
slQuaternion* slQuaternionInverse(slQuaternion* pOut,
											const slQuaternion* pIn)
{
	slScalar l = slQuaternionLength(pIn);

	if (slAbs(l) > slEpsilon)
	{
		pOut->m_X = 0.0;
		pOut->m_Y = 0.0;
		pOut->m_Z = 0.0;
		pOut->m_W = 0.0;

		return pOut;
	}

	slQuaternion* tmp = 0;

	///Get the conjugute and divide by the length
	slQuaternionScale(pOut,
				slQuaternionConjugate(tmp, pIn), 1.0 / l);

	return pOut;
}

///< Returns true if the quaternion is an identity quaternion
bool slQuaternionIsIdentity(const slQuaternion* pIn)
{
	return (pIn->m_X == 0.0 && pIn->m_Y == 0.0 && pIn->m_Z == 0.0 &&
				pIn->m_W == 1.0);
}

///< Returns the length of the quaternion
slScalar slQuaternionLength(const slQuaternion* pIn)
{
	return slSqrt(slQuaternionLengthSq(pIn));
}

///< Returns the length of the quaternion squared (prevents a sqrt)
slScalar slQuaternionLengthSq(const slQuaternion* pIn)
{
	return pIn->m_X * pIn->m_X + pIn->m_Y * pIn->m_Y +
						pIn->m_Z * pIn->m_Z + pIn->m_W * pIn->m_W;
}

///< Returns the natural logarithm
slQuaternion* slQuaternionLn(slQuaternion* pOut,
										const slQuaternion* pIn)
{
	/*
		A unit quaternion, is defined by:
		Q == (cos(theta), sin(theta) * v) where |v| = 1
		The natural logarithm of Q is, ln(Q) = (0, theta * v)
	*/

	assert(0);

	return pOut;
}

///< Multiplies 2 quaternions together
extern
slQuaternion* slQuaternionMultiply(slQuaternion* pOut,
								 const slQuaternion* q1,
								 const slQuaternion* q2)
{
	pOut->m_W = q1->m_W * q2->m_W - q1->m_X * q2->m_X - q1->m_Y * q2->m_Y - q1->m_Z * q2->m_Z;
	pOut->m_X = q1->m_W * q2->m_X + q1->m_X * q2->m_W + q1->m_Y * q2->m_Z - q1->m_Z * q2->m_Y;
	pOut->m_Y = q1->m_W * q2->m_Y + q1->m_Y * q2->m_W + q1->m_Z * q2->m_X - q1->m_X * q2->m_Z;
	pOut->m_Z = q1->m_W * q2->m_Z + q1->m_Z * q2->m_W + q1->m_X * q2->m_Y - q1->m_Y * q2->m_X;

	return pOut;
}

///< Normalizes a quaternion
slQuaternion* slQuaternionNormalize(slQuaternion* pOut,
											const slQuaternion* pIn)
{
	slScalar length = slQuaternionLength(pIn);
	assert(slAbs(length) > slEpsilon);
	slQuaternionScale(pOut, pIn, 1.0f / length);

	return pOut;
}

///< Rotates a quaternion around an axis
slQuaternion* slQuaternionRotationAxis(slQuaternion* pOut,
									const slVec3* pV,
									slScalar angle)
{
	slScalar rad = angle / 2;
	slScalar scale	= slSin(rad);

	pOut->m_W = slCos(rad);
	pOut->m_X = pV->m_X * scale;
	pOut->m_Y = pV->m_Y * scale;
	pOut->m_Z = pV->m_Z * scale;

	slQuaternionNormalize(pOut, pOut);

	return pOut;
}

///< Creates a quaternion from a rotation matrix
slQuaternion* slQuaternionRotationMatrix(slQuaternion* pOut,
										const slMat4* pIn)
{
/*
Note: The OpenGL matrices are transposed from the description below
taken from the Matrix and Quaternion FAQ

    if ( mat[0] > mat[5] && mat[0] > mat[10] )  {	// Column 0:
        S  = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
        X = 0.25 * S;
        Y = (mat[4] + mat[1] ) / S;
        Z = (mat[2] + mat[8] ) / S;
        W = (mat[9] - mat[6] ) / S;
    } else if ( mat[5] > mat[10] ) {			// Column 1:
        S  = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
        X = (mat[4] + mat[1] ) / S;
        Y = 0.25 * S;
        Z = (mat[9] + mat[6] ) / S;
        W = (mat[2] - mat[8] ) / S;
    } else {						// Column 2:
        S  = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
        X = (mat[2] + mat[8] ) / S;
        Y = (mat[9] + mat[6] ) / S;
        Z = 0.25 * S;
        W = (mat[4] - mat[1] ) / S;
    }
*/

	slScalar T = pIn->m_Mat[0] + pIn->m_Mat[5] + pIn->m_Mat[10];

	if (T > slEpsilon) {
		//If the trace is greater than zero we always use this calculation:
		/*  S = sqrt(T) * 2;
		  X = ( mat[9] - mat[6] ) / S;
		  Y = ( mat[2] - mat[8] ) / S;
		  Z = ( mat[4] - mat[1] ) / S;
		  W = 0.25 * S;*/

		slScalar s = slSqrt(T) * 2;
		pOut->m_X = (pIn->m_Mat[9] - pIn->m_Mat[6]) / s;
		pOut->m_Y = (pIn->m_Mat[8] - pIn->m_Mat[2]) / s;
		pOut->m_Z = (pIn->m_Mat[1] - pIn->m_Mat[4]) / s;
		pOut->m_W = 0.25 * s;

		slQuaternionNormalize(pOut, pOut);
		return pOut;
	}

	//Otherwise the calculation depends on which major diagonal element has the greatest value.

	if (pIn->m_Mat[0] > pIn->m_Mat[5] && pIn->m_Mat[0] > pIn->m_Mat[10]) {
		slScalar s = slSqrt(1 + pIn->m_Mat[0] - pIn->m_Mat[5] - pIn->m_Mat[10]) * 2;
		pOut->m_X = 0.25 * s;
		pOut->m_Y = (pIn->m_Mat[1] + pIn->m_Mat[4]) / s;
		pOut->m_Z = (pIn->m_Mat[8] + pIn->m_Mat[2]) / s;
		pOut->m_W = (pIn->m_Mat[9] - pIn->m_Mat[6]) / s;
	}
	else if (pIn->m_Mat[5] > pIn->m_Mat[10]) {
		slScalar s = slSqrt(1 + pIn->m_Mat[5] - pIn->m_Mat[0] - pIn->m_Mat[10]) * 2;
		pOut->m_X = (pIn->m_Mat[1] + pIn->m_Mat[4]) / s;
		pOut->m_Y = 0.25 * s;
		pOut->m_Z = (pIn->m_Mat[9] + pIn->m_Mat[6]) / s;
		pOut->m_W = (pIn->m_Mat[8] - pIn->m_Mat[2]) / s;
	}
	else {
		slScalar s = sqrt(1 + pIn->m_Mat[10] - pIn->m_Mat[0] - pIn->m_Mat[5]) * 2;
        pOut->m_X = (pIn->m_Mat[8] + pIn->m_Mat[2] ) / s;
        pOut->m_Y = (pIn->m_Mat[6] + pIn->m_Mat[9] ) / s;
        pOut->m_Z = 0.25 * s;
        pOut->m_W = (pIn->m_Mat[1] - pIn->m_Mat[4] ) / s;
	}

	slQuaternionNormalize(pOut, pOut);
	return pOut;
}

///< Create a quaternion from yaw, pitch and roll
slQuaternion* slQuaternionRotationYawPitchRoll(slQuaternion* pOut,
												slScalar yaw,
												slScalar pitch,
												slScalar roll)
{
	slScalar	ex, ey, ez;		// temp half euler angles
	slScalar	cr, cp, cy, sr, sp, sy, cpcy, spsy;		// temp vars in roll,pitch yaw

	ex = slDegreesToRadians(pitch) / 2.0;	// convert to rads and half them
	ey = slDegreesToRadians(yaw) / 2.0;
	ez = slDegreesToRadians(roll) / 2.0;

	cr = slCos(ex);
	cp = slCos(ey);
	cy = slCos(ez);

	sr = slSin(ex);
	sp = slSin(ey);
	sy = slSin(ez);

	cpcy = cp * cy;
	spsy = sp * sy;

	pOut->m_W = cr * cpcy + sr * spsy;

	pOut->m_X = sr * cpcy - cr * spsy;
	pOut->m_Y = cr * sp * cy + sr * cp * sy;
	pOut->m_Z = cr * cp * sy - sr * sp * cy;

	slQuaternionNormalize(pOut, pOut);

	return pOut;
}

///< Interpolate between 2 quaternions
slQuaternion* slQuaternionSlerp(slQuaternion* pOut,
								const slQuaternion* q1,
								const slQuaternion* q2,
								slScalar t)
{

 /*float CosTheta = Q0.DotProd(Q1);
  float Theta = acosf(CosTheta);
  float SinTheta = sqrtf(1.0f-CosTheta*CosTheta);

  float Sin_T_Theta = sinf(T*Theta)/SinTheta;
  float Sin_OneMinusT_Theta = sinf((1.0f-T)*Theta)/SinTheta;

  Quaternion Result = Q0*Sin_OneMinusT_Theta;
  Result += (Q1*Sin_T_Theta);

  return Result;*/

	slScalar ct = slQuaternionDot(q1, q2);
	slScalar theta = slACos(ct);
	slScalar st = slSqrt(1.0 - slSQR(ct));

	slScalar stt = slSin(t * theta) / st;
	slScalar somt = slSin((1.0 - t) * theta) / st;

	slQuaternion temp, temp2;
	slQuaternionScale(&temp, q1, somt);
	slQuaternionScale(&temp2, q2, stt);
	slQuaternionAdd(pOut, &temp, &temp2);

	return pOut;
}

///< Get the axis and angle of rotation from a quaternion
void slQuaternionToAxisAngle(const slQuaternion* pIn,
								slVec3* pAxis,
								slScalar* pAngle)
{
	slScalar 	tempAngle;		// temp angle
	slScalar	scale;			// temp vars

	tempAngle = slACos(pIn->m_W);
	scale = slSqrt(slSQR(pIn->m_X) + slSQR(pIn->m_Y) + slSQR(pIn->m_Z));

	if (scale > -slEpsilon && scale < slEpsilon
		|| scale < 360.0f + slEpsilon && scale > 360.0f - slEpsilon)		// angle is 0 or 360 so just simply set axis to 0,0,1 with angle 0
	{
		*pAngle = 0.0f;

		pAxis->m_X = 0.0f;
		pAxis->m_Y = 0.0f;
		pAxis->m_Z = 1.0f;
	}
	else
	{
		*pAngle = tempAngle * 2.0;		// angle in radians

		pAxis->m_X = pIn->m_X / scale;
		pAxis->m_Y = pIn->m_Y / scale;
		pAxis->m_Z = pIn->m_Z / scale;
		slVec3Normalize(pAxis, pAxis);
	}
}

slQuaternion* slQuaternionScale(slQuaternion* pOut,
										const slQuaternion* pIn,
										slScalar s)
{
	pOut->m_X = pIn->m_X * s;
	pOut->m_Y = pIn->m_Y * s;
	pOut->m_Z = pIn->m_Z * s;
	pOut->m_W = pIn->m_W * s;

	return pOut;
}

slQuaternion* slQuaternionAssign(slQuaternion* pOut, const slQuaternion* pIn)
{
	memcpy(pOut, pIn, sizeof(float) * 4);

	return pOut;
}

slQuaternion* slQuaternionAdd(slQuaternion* pOut, const slQuaternion* pQ1, const slQuaternion* pQ2)
{
	pOut->m_X = pQ1->m_X + pQ2->m_X;
	pOut->m_Y = pQ1->m_Y + pQ2->m_Y;
	pOut->m_Z = pQ1->m_Z + pQ2->m_Z;
	pOut->m_W = pQ1->m_W + pQ2->m_W;

	return pOut;
}
